#include "ros/ros.h"
#include "std_msgs/Float32MultiArray.h"
#include <string>
#include <iostream>
#include <cstdio>
#include <math.h>
#include <sstream>
#include <vector>
#include "serial/serial.h"
#include <unistd.h>
#include <time.h>
#include <pthread.h>
#include "transformer/transformer.h"
#include <gps_driver/gps.h>

using namespace std;

using std::string;
using std::exception;
using std::cout;
using std::cerr;
using std::endl;
using std::vector;

serial::Serial *my_serial;
struct gps_info{
  string time;
  double wd;
  double jd;
  int state;
  int star;
  double height;
};

void print_usage()
{
    cerr << "Usage: test_serial <serial port address> ";
    cerr << "<baudrate>" << endl;
}

void SplitString(const std::string& s, std::vector<std::string>& v, const std::string& c)
{
  std::string::size_type pos1, pos2;
  pos2 = s.find(c);
  pos1 = 0;
  while(std::string::npos != pos2)
  {
    v.push_back(s.substr(pos1, pos2-pos1));
 
    pos1 = pos2 + c.size();
    pos2 = s.find(c, pos1);
  }
  if(pos1 != s.length())
    v.push_back(s.substr(pos1));
}

double str2double(string& s)
{
  double d;
  istringstream istrStream(s);
  istrStream >> d;
  return d;
}

int str2int(string& s)
{
  int i;
  istringstream istrStream(s);
  istrStream >> i;
  return i;
}

int main(int argc, char **argv) {
  ros::init(argc, argv, "gps_driver");
  ros::NodeHandle nh;
  ros::Publisher gps_pub=nh.advertise<gps_driver::gps>("gps_data",1000);
  gps_driver::gps m_gps;

  if(argc < 3) {
    print_usage();
    return 0;
  }
  // Argument 1 is the port
  string port(argv[1]);
  // Argument 2 is the baudrate
  unsigned long baud = 0;
  sscanf(argv[2], "%lu", &baud);
  cout << "port:" << port << " baud:" << baud << endl;

  // port, baudrate, timeout in milliseconds
  my_serial = new serial::Serial(port, baud, serial::Timeout::simpleTimeout(5));
  gps_info m_gps_info;
  PCRDCARTESIAN xy;    //space rect coodinate
  PCRDGEODETIC jw;    //jd & wd

  if(my_serial->isOpen())
  {
    cout << " Open Device Normally." << endl;
    cout << "----------------------------" << endl;
  }
  else
    cout << " Failed To Open Device." << endl;

  string buffer_rx = "";
  string gga = "";
  string result = "";
  string head = "$GPGGA";
  ros::Rate loop_rate(5);  //以5帧的频率监控串口
  while (my_serial->isOpen()&&ros::ok()) {
    //从串口缓冲区读取500字节的数据并存储到result中
    result = my_serial->read(500);  
    //判断：如果读取到数据则写入到buffer_rx中，然后跳过之后的循环重复读取
    if(result.size()>0)
    {
        buffer_rx+=result;
        continue;
    }
    //判断：如果读取不到新的数据，并且buffer_rx不为空，则认为该帧数据已经读取完毕
    if(buffer_rx.size() > 0 && result.size() == 0)
    {
        int ind_start = buffer_rx.find(head);
	if(ind_start == string::npos)
	{
		cout << "try finding..." << endl;
		buffer_rx="";
		continue;
	}
        int ind_end = -1;
        int sub_len = 0;
        for(int i=ind_start+1; i<buffer_rx.size()-ind_start; i++)
        {
          if(buffer_rx[i]=='$')
          {
            ind_end = i-1;
            sub_len = ind_end - ind_start + 1;
            gga = buffer_rx.substr(ind_start,sub_len);
            cout << gga;
            std::vector<std::string> element_str;
            SplitString(gga, element_str, ",");
            m_gps_info.time = element_str[1];
            m_gps_info.wd = str2double(element_str[2]);
            m_gps_info.jd = str2double(element_str[4]);
            m_gps_info.state = str2int(element_str[6]);
            m_gps_info.star = str2int(element_str[7]);
            m_gps_info.height = str2double(element_str[9]);
            jw.longitude = (m_gps_info.jd - floor(m_gps_info.jd)) / 60.0 + floor(m_gps_info.jd);
            jw.latitude = (m_gps_info.wd - floor(m_gps_info.wd)) / 60.0 + floor(m_gps_info.wd);
            jw.height = m_gps_info.height;

            GeodeticToCartesian(xy, jw);

            m_gps.header.stamp = ros::Time::now();
            m_gps.header.frame_id = "world";
            m_gps.time = m_gps_info.time;
            m_gps.wd = m_gps_info.wd;
            m_gps.jd = m_gps_info.jd;
            m_gps.height = m_gps_info.height;
            m_gps.state = m_gps_info.state;
            m_gps.star = m_gps_info.star;
            m_gps.x = xy.x;
            m_gps.y = xy.y;
            m_gps.z = m_gps_info.height;

            gps_pub.publish(m_gps);

            break;
          }
        }

        buffer_rx="";
    }
    loop_rate.sleep();
    ros::spinOnce();
  }

  //qxwz_rtcm_stop();
  //printf("qxwz_rtcm_stop done\r\n");

  return 0; 
}
